/**
    Copyright 2015 Table Rider
    By: Anders Nylander
*/

#include "SerialCom.h"

SerialCom::SerialCom(){
    fd = serialport_init("/dev/ttymxc3", 115200);
}

SerialCom::~SerialCom(){
    serialport_close(fd);
}

int SerialCom::getfd(){
    return fd;
}

const char* SerialCom::send_msg(void* msg){
    unsigned short* temp = (unsigned short*) msg;
    unsigned short Cmd = temp[0];
    unsigned char* b = (unsigned char*) msg;
    static char buf[256];
    int i;
    for(i=0; i<256; i++)
    {
        buf[i] = '\0';
    }

    switch(Cmd)
    {
    case 0: break;
    case TBLRDR_PING:{
        if (John){
        //    return "This is John!";
        }
       // else return "This is Doe!";
        //printf("Doing a ping!\n");
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}

        break;
        }
    case TBLRDR_GETPOS:{
        //printf("Getting position...\n");
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        break;
        }
    case TBLRDR_SETPOS:{
        //printf("Setting position...\n");
        serialport_writebyte(fd, 10);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        for(i=0;i<8;i++){
            serialport_writebyte(fd, *(b+2+i));
            //printf("%d ", *(b+2+i));
        }
        break;
        }
    case TBLRDR_GETANGLE:{
        //printf("Getting angle...\n");
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        break;
        }
    case TBLRDR_SETANGLE:{
        //printf("Setting angle...\n");
        serialport_writebyte(fd, 6);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        for(i=0;i<4;i++) serialport_writebyte(fd, *(b+2+i));
        //for(i=0;i<6;i++) printf("%d ", b[i]);
        break;
        }
    case TBLRDR_MOVETO:{
        //printf("Moving to...\n");
        serialport_writebyte(fd, 10);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        for(i=0;i<8;i++) serialport_writebyte(fd, *(b+2+i));
        break;
        }
    case TBLRDR_MOVEBY:{
        //printf("Moving by...\n");
        serialport_writebyte(fd, 10);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        for(i=0;i<8;i++) serialport_writebyte(fd, *(b+2+i));
        break;
        }
    case TBLRDR_JOHN:{
        //printf("Identifying John...\n");
        serialport_writebyte(fd, 6);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        for(i=0;i<4;i++) serialport_writebyte(fd, *(b+2+i));
        break;
        }
    case TBLRDR_MOVEFWD:{
        //printf("Moving forward...\n");
        serialport_writebyte(fd, 6);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        for(i=0;i<4;i++) serialport_writebyte(fd, *(b+2+i));
        break;
        }
    case TBLRDR_TURNTO:{
        //printf("Turning to...\n");
        serialport_writebyte(fd, 6);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        for(i=0;i<4;i++) serialport_writebyte(fd, *(b+2+i));
        break;
        }
    case TBLRDR_TURNBY:{
        //printf("Turning by...\n");
        serialport_writebyte(fd, 6);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        for(i=0;i<4;i++) serialport_writebyte(fd, *(b+2+i));
        break;
        }
    case TBLRDR_GETENC:{
        //printf("Getting enc...\n");
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        break;
        }
    case TBLRDR_GETDIST:{
        //printf("Getting distance...\n");
        serialport_writebyte(fd, 3);
        for(i=0;i<3;i++){ serialport_writebyte(fd, b[i]);}
        break;
    }
    case TBLRDR_NEXTSTEP:
    {
        //printf("Doing next step...\n");
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        break;
    }
    case TBLRDR_ALLSTEPS:
    {
        //printf("Doing all steps...\n");
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        break;
    }
    case TBLRDR_START:
    {
        //printf("Starting...\n");
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        break;
    }
    case TBLRDR_STOP:
    {
        //printf("Stopping...\n");
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        break;
    }
    case TBLRDR_UPDATE:
    {
        //printf("Updating...\n");
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        break;
    }
    case TBLRDR_GETCUR:
    {
        //printf("Getting raw current...\n");
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        break;
    }
    case TBLRDR_GETRCR:
    {
        //printf("Getting real current...\n");
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        break;
    }
    case TBLRDR_CONTROLINIT:{
        serialport_writebyte(fd, 10);
        for(i=0;i<10;i++){ serialport_writebyte(fd, b[i]);}
        break;
    }
    case TBLRDR_CONTROLDONE:{
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        break;
    }
    case TBLRDR_TESTLMOTOR:{
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        break;
    }
    case TBLRDR_TESTRMOTOR:{
        serialport_writebyte(fd, 2);
        for(i=0;i<2;i++){ serialport_writebyte(fd, b[i]);}
        break;
    }
    default: break;
    }

    if (serialport_read_until(fd, buf, '\255', 256, 1000) == -1) return "Couldn't read\n";

    return buf;
}
